#include <motion_controller/controller/controller_p2p.h>


// 构造函数
ControllerP2P::ControllerP2P(boost::shared_ptr<PlannerBridge> planner_bridge_ptr,
    boost::shared_ptr<SensorBridge> sensor_bridge_ptr):
    __goal_pose_ptr(nullptr),
    __planner_bridge_ptr(planner_bridge_ptr),
    __sensor_bridge_ptr(sensor_bridge_ptr){
        __rotate_start_direction = false;
}

// 析构函数
ControllerP2P::~ControllerP2P(){

}

// 设置数据
void ControllerP2P::SetData(vector<Pose2D> points){
    if(!points.empty()){
        __goal_pose_ptr = boost::make_shared<Pose2D>(points.front());
    }
}

// 空闲状态
void ControllerP2P::ResetIdle(){
    __global_path.clear();
}

// 执行中预处理函数
void ControllerP2P::ExecutingEntry(){
    if(!__goal_pose_ptr){
        LOG(WARNING) << "__goal_pose_ptr is nullptr!";
        SetEvent(ControllerState::Event::EV_CANCEL);
        return;
    }
    if(!__planner_bridge_ptr){
        LOG(WARNING) << "planner_ptr is nullptr!";
        SetEvent(ControllerState::Event::EV_CANCEL);
        return;
    }

    // 生成路径
    Pose2D start = msg_convert::Pose2Pose2D(__global_robot_pose.pose);
    Pose2D goal = *__goal_pose_ptr;
    bool gen_state = __planner_bridge_ptr->GenerateP2PPath(start, goal, __global_path);
    __rotate_start_direction = false;
    if(!gen_state || __global_path.empty()){
        LOG(WARNING) << "generate path failed!";
        SetEvent(ControllerState::Event::EV_CANCEL);
        return;    
    }
    cout << " global_path front: " << __global_path.front() << 
        ", back: " << __global_path.back() << endl;
}

// 执行中处理函数
void ControllerP2P::ExecutingRunning(){
    try{
        Velocity2D cmd_vel;

        if(__global_path.empty()){
            throw std::runtime_error("__global_path is empty!");
        }
        Pose2D curr = msg_convert::Pose2Pose2D(__global_robot_pose.pose);
        if(__planner_bridge_ptr->IsPositionReached(curr, __global_path.back(), 0.2)){
            ROS_INFO("The robot reaches the goal point.");
            __planner_bridge_ptr->PublishTwist();
            
            SetEvent(ControllerState::Event::EV_OVER);
            return;
        }

        // 全局路径转局部路径 
        __planner_bridge_ptr->PathGlobal2Local(curr, __global_path, __local_path, 0.2);
        
        // 获取前瞻目标点
        Pose2D local_goal, global_goal;
        if(!__planner_bridge_ptr->FindLookAheadPointByDistance(__local_path, curr, local_goal, 0.6)){
            throw std::runtime_error("can not get target point!");
        }
        global_goal = math_utils::Pose2DLocal2Global(curr, local_goal);
        cout << "curr: " << curr 
            << "local_goal: " << local_goal 
            << ", global_goal: " << global_goal << endl;


        // 起点角度旋转, 减少不必要转弯次数
        if(!__rotate_start_direction){
            cout << "__rotate_start_direction: " << __rotate_start_direction << endl;
            if(!__planner_bridge_ptr->IsAngleReached(curr, global_goal, 0.2)){
                __planner_bridge_ptr->RotateToAngle(cmd_vel, curr, global_goal);
                __planner_bridge_ptr->PublishTwist(cmd_vel);
                cout << "cmd_vel: " << cmd_vel << endl;
            }
            else{
                __rotate_start_direction = true;
                __planner_bridge_ptr->PublishTwist();
            }
            return;
        }

        // 障碍物检测，计算最佳速度
        LidarScan2D lidar_scan_2d;
        if(!__sensor_bridge_ptr->GetLidarData(lidar_scan_2d)){
            LOG(WARNING) << "can not get lidar data!";
        }
        if(!__planner_bridge_ptr->ComputeTwist(cmd_vel, lidar_scan_2d, curr, local_goal)){
            throw std::runtime_error("can not get cmd_vel!");
        }

        // 速度指令控制
        __planner_bridge_ptr->PublishTwist(cmd_vel);
    }
    catch (const std::runtime_error& e) {
        std::cerr << "[ControllerP2P | ExecutingRunning] error: " << e.what() << std::endl;
    }

}

